﻿using System;
using System.Collections.Generic;
using System.Text;
using PositionSource.WebCam.Interfaces;
using PositionSource.LedPositionExtractor;
using RoverStudio.Common.PositionSource;
using RoverStudio.Math;


namespace PositionSource.WebCam
{
    public class WebcamLEDPositionSource : IPositionSource
    {
        #region IPositionSource Members

        public event PositionUpdatedHandler PositionUpdated;

        public void Start()
        {
            throw new NotImplementedException();
        }

        public void Stop()
        {
            throw new NotImplementedException();
        }

        #endregion

        //public event CameraEventHandler NewFrame;
        public event NewFrameDelegate NewFrame;

        private ILedPositionExtractor ledPositionExtractor;

        public ILedPositionExtractor LedPositionExtractor
        {
            get { return ledPositionExtractor; }
            set { ledPositionExtractor = value; }
        }

        private ITransformationCalculator transformationCalculator; // = new World3DFromPicture2D();

        public ITransformationCalculator TransformationCalculator
        {
            get { return transformationCalculator; }
            set { transformationCalculator = value; }
        }


        private IVideoSourceCommon videoSource;

        public IVideoSourceCommon VideoSource
        {
            get { return videoSource; }
            //set { videoSource = value; }
        }

        public WebcamLEDPositionSource(IVideoSourceCommon videoSource)
        {
            this.videoSource = videoSource;
            ledPositionExtractor = new LedDetector();
            videoSource.NewFrame += new NewFrameDelegate(videoSource_FrameGrabbed);
        
            //videoSource.Start();

        }

        private Vector3D refPoint = new Vector3D();
        private Rotation refRotationInv = new Rotation();
        private bool setreference = false;

        private int fta = 1;

        public void SetReferenceTransformation()
        {
            fta = ((LedDetector)ledPositionExtractor).FramesToAverage;
            ((LedDetector)ledPositionExtractor).FramesToAverage = 5;
            setreference = true;
            Console.WriteLine("Setting reference...");
        }

        public void SetReferenceTransformation(Vector3D translation, Rotation rotation)
        {
            refPoint = translation;
            refRotationInv = rotation.Inverse();
        }

        //void videoSource_NewFrame(object sender, CameraEventArgs e)
        //{
        //    try
        //    {
        //        videoSource_FrameGrabbed(e.Bitmap);
        //    }
        //    catch(Exception ex)
        //    {
        //        Console.WriteLine(ex);
        //    }
        //}

        void videoSource_FrameGrabbed(System.Drawing.Bitmap frame)
        {
            DateTime time = DateTime.Now;
            Vector2D[] leds = ledPositionExtractor.GetLEDPositionsFromImage(ref frame);
            if (NewFrame != null)
                NewFrame(frame);
            if (leds == null) return;

#if DEBUG
            for (int i = 0; i < leds.Length; i++)
                Console.WriteLine("LED {0}: {1}", i, leds[i].ToString());
#endif

            Vector3D position;
            Rotation orientation;
            transformationCalculator.CalculateTransformation(leds, out position, out orientation);

            if (setreference)
            {
                setreference = false;
                refPoint = new Vector3D(position);
                refRotationInv = orientation.Inverse();
                ((LedDetector)ledPositionExtractor).FramesToAverage = fta;
            }

            position = refRotationInv.Apply(position - refPoint);
            orientation = refRotationInv * orientation;

#if DEBUG
            Console.WriteLine("Position: ({0:00.00}; {1:00.00}; {2:00.00})", position.X, position.Y, position.Z);
            Console.WriteLine("Orientation: ({0:00.00}; {1:00.00}; {2:00.00})", orientation.YAxis.X, orientation.YAxis.Y, orientation.YAxis.Z);
#endif
            if (PositionUpdated != null)
                PositionUpdated(position, orientation, time);
        }



    }
}
